#include <buola/cv/geometry/transform.h>
#include <buola/mat/cquat.h>
#include <buola/mat/transformations.h>
#include <buola/mat/eigen.h>
#include <opencv2/calib3d/calib3d.hpp>

namespace buola { namespace cvi {
    
mat::CMat34d get_rigid_transform(const std::vector<mat::CVec3d> &pSrc,const std::vector<mat::CVec3d> &pDst)
{
    if(pSrc.size()<3||pSrc.size()!=pDst.size())
        throw XArgument("invalid parameters to get_rigid_transform");
    
    mat::CVec3d lSrcCentroid=std::accumulate(pSrc.begin(),pSrc.end(),mat::CVec3d(0,0,0))/pSrc.size();
    mat::CVec3d lDstCentroid=std::accumulate(pDst.begin(),pDst.end(),mat::CVec3d(0,0,0))/pDst.size();
    
    mat::CMat3d lMMat(nZero);
    
    mat::CQuat_d lRot;

//    if(pSrc.size()>3)
    {
        for(int i=0;i<pSrc.size();i++)
        {
            lMMat+=(pSrc[i]-lSrcCentroid)*(pDst[i]-lDstCentroid).T();
        }
        
        mat::CMat4d lNMat{lMMat(0,0)+lMMat(1,1)+lMMat(2,2),  lMMat(1,2)-lMMat(2,1),  lMMat(2,0)-lMMat(0,2),  lMMat(0,1)-lMMat(1,0),
                        lMMat(1,2)-lMMat(2,1),  lMMat(0,0)-lMMat(1,1)-lMMat(2,2),  lMMat(0,1)+lMMat(1,0),  lMMat(2,0)+lMMat(0,2),
                        lMMat(2,0)-lMMat(0,2),  lMMat(0,1)+lMMat(1,0),  -lMMat(0,0)+lMMat(1,1)-lMMat(2,2), lMMat(1,2)+lMMat(2,1),
                        lMMat(0,1)-lMMat(1,0),  lMMat(2,0)+lMMat(0,2),  lMMat(1,2)+lMMat(2,1), -lMMat(0,0)-lMMat(1,1)+lMMat(2,2)};
        
        mat::CVec4d lEigenValues;
        mat::CMat4d lEigenVectors;

        std::tie(lEigenValues,lEigenVectors)=mat::eigen_vectors(lNMat);
        int lMaxInd=indmax(lEigenValues);
        
        lRot=mat::CQuat_d(lEigenVectors(0,lMaxInd),lEigenVectors(1,lMaxInd),lEigenVectors(2,lMaxInd),lEigenVectors(3,lMaxInd),true);
    }
//     else
//     {
//         mat::CVec3d lS1=pSrc[0]-lSrcCentroid,lS2=pSrc[1]-lSrcCentroid,lS3=pSrc[2]-lSrcCentroid;
//         mat::CVec3d lD1=pDst[0]-lDstCentroid,lD2=pDst[1]-lDstCentroid,lD3=pDst[2]-lDstCentroid;
//         //normals in each plane
//         mat::CVec3d lNs=cross(lS1,lS2),lNd=cross(lD1,lD2);
//         lNs/=norm(lNs);
//         lNd/=norm(lNd);
//         
//         mat::CQuat_d lQuatA(lNs,lNd);
//         
//         lS1=lQuatA*lS1;
//         lS2=lQuatA*lS2;
//         lS3=lQuatA*lS3;
//         
//         double C=dot(lD1,lS1)+dot(lD2,lS2)+dot(lD3,lS3);
//         double S=dot(cross(lD1,lS1)+cross(lD2,lS2)+cross(lD3,lS3),lNd);
//         
//         double lSinPhi=S/sqrt(sq(S)+sq(C));
//         double lCosPhi=C/sqrt(sq(S)+sq(C));
//         double lCosPhi_2=sqrt(0.5+0.5*lCosPhi);
//         double lSinPhi_2=lSinPhi/sqrt(2+2*lCosPhi);
//         mat::CQuat_d lQuatP(lCosPhi_2,lSinPhi_2*lNd.x(),lSinPhi_2*lNd.y(),lSinPhi_2*lNd.z());
//         lRot=lQuatP*lQuatA;
//     }

    mat::CVec3d lTrans=lDstCentroid-lRot*lSrcCentroid;

    return rotation_translation_3d(lRot,lTrans);
}
    
mat::CMat34d estimate_affine_transform(const std::vector<mat::CVec3d> &pSrc,const std::vector<mat::CVec3d> &pDst,
                                       double pTolerance,double pConfidence)
{
    std::vector<cv::Point3f> lSrc,lDst;
    
    for(int i=0;i<pSrc.size();i++)
    {
        lSrc.emplace_back(pSrc[i].x(),pSrc[i].y(),pSrc[i].z());
        lDst.emplace_back(pDst[i].x(),pDst[i].y(),pDst[i].z());
    }

    cv::Mat lResult,lInliers;
    cv::estimateAffine3D(lSrc,lDst,lResult,lInliers,pTolerance,pConfidence);

    return mat::CMat34d((double*)lResult.data);
}

mat::CMat34d estimate_rigid_transform(const std::vector<mat::CVec3d> &pSrc,const std::vector<mat::CVec3d> &pDst,
                                       double pTolerance,double pConfidence)
{
    if(pSrc.size()<3) throw buola::XArgument("not enough points in estimate_rigid_transform");
    
    double lTolerance2=sq(pTolerance);
    
    std::array<int,3> lBestIndices;
    int lBest=0;
    
    for(int i=0;i<1000;i++)
    {
        std::array<int,3> lIndices;
        for(int j=0;j<3;j++)
        {
            lIndices[j]=random::get<int>(pSrc.size()-1);
            for(int k=0;k<j;k++)
            {
                if(lIndices[j]==lIndices[k])
                {
                    j--;
                    break;
                }
            }   
        }
        
        std::vector<mat::CVec3d> lSrcSet,lDstSet;
        
        for(int j=0;j<3;j++)
        {
            lSrcSet.push_back(pSrc[lIndices[j]]);
            lDstSet.push_back(pDst[lIndices[j]]);
        }
        
        mat::CMat34d lTrans=get_rigid_transform(lSrcSet,lDstSet);
        
        int lInliers=0;
        
        for(int j=0;j<pSrc.size();j++)
        {
            if(norm2(pDst[j]-lTrans*pSrc[j])<lTolerance2)
                lInliers++;
        }
        
        if(lInliers>lBest)
        {
            lBestIndices=lIndices;
            lBest=lInliers;
            if(lInliers>pSrc.size()/2) break;
        }
    }
    
    {
        std::vector<mat::CVec3d> lSrcSet,lDstSet;
        
        for(int j=0;j<3;j++)
        {
            lSrcSet.push_back(pSrc[lBestIndices[j]]);
            lDstSet.push_back(pDst[lBestIndices[j]]);
        }
        
        mat::CMat34d lTrans=get_rigid_transform(lSrcSet,lDstSet);
        
        std::vector<mat::CVec3d> lSrcFinal,lDstFinal;
        
        for(int j=0;j<pSrc.size();j++)
        {
            if(norm2(pDst[j]-lTrans*pSrc[j])<lTolerance2)
            {
                lSrcFinal.push_back(pSrc[j]);
                lDstFinal.push_back(pDst[j]);
            }
        }
        
        return get_rigid_transform(lSrcFinal,lDstFinal);
    }
}

/*namespace cvi*/ } /*namespace buola*/ }
